/*
frigOS
Licensed under GPLv3
*/

#include "usart0.h"
#include "serial.h"
#include "adc.h"
#include "ax12.h"
#include "axs1.h"
#include "dynamixel.h"
#include "led.h"
#include "protocol.h"
#include "kernel.h"
#include "shared_memory.h"

#include <avr/interrupt.h>
#include <util/delay.h>

volatile Semaphore responsePacketLock;
volatile DXL_Packet responsePacket;

volatile uint8_t rxBuffer_USART0[USART0_RX_BUFFER_SIZE];
volatile uint8_t txBuffer_USART0[USART0_TX_BUFFER_SIZE];
volatile Semaphore usart0_lock;

volatile IO_Queue rxQueue_USART0;
volatile IO_Queue txQueue_USART0;

// initialize USART0 for use with the AX-12 servo motors
// defaults into Rx mode after initialization
void initUSART0(void)
{
    /* copied from AX-12 datasheet sample code

    UBRR0H = 0;
    UBRR0L = bBaudrate; // bBaudrate = 1 in sample code
    UCSR0A = 0x02;      // == (1 << U2X0)
    UCSR0B = 0x18;      // == (1 << RXEN0) | (1 << RXB80)
    if(bInterrupt&RX_INTERRUPT)
        sbi(UCSR0B,7);  // UCSR0B |= (1 << RXCIE0)
    UCSR0C = 0x06;      // == (1 << UCSZ01) | (1 << UCSZ00)
    UDR0 = 0xFF;
    sbi(UCSR0A,6);      // UCSR0A |= (1 << TXC0)
    */

    // double usart transmission speed
    UCSR0A = (1 << U2X0);

    // rx complete interrupt
    // tx complete interrupt
    // receiver
    // transmitter
    UCSR0B = (1 << RXCIE0) | (1 << TXCIE0) | (1 << RXEN0) | (1 << TXEN0);

    // 8N1
    // asynchronous
    // parity disabled
    // 1 stop bit
    // character size 8 bit
    UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);

    // baud rate
    UBRR0H = (uint8_t) USART0_BAUD_RATE_HI;
    UBRR0L = (uint8_t) USART0_BAUD_RATE_LO;

    //UDR0 = 0xff;

    setRxUSART0();

    initSemaphore(&usart0_lock, 1);
    initSemaphore(&responsePacketLock, 1);

    // initialize the rx and tx queues
    rxQueue_USART0.buffer = rxBuffer_USART0;
    rxQueue_USART0.maxLength = USART0_RX_BUFFER_SIZE;
    rxQueue_USART0.queueLength = 0;
    rxQueue_USART0.empty = TRUE;
    rxQueue_USART0.full = FALSE;
    rxQueue_USART0.txOnEnqueue = FALSE;
    rxQueue_USART0.in = 0;
    rxQueue_USART0.out = 0;
    rxQueue_USART0.txOn = TX_ON_NONE;

    txQueue_USART0.buffer = txBuffer_USART0;
    txQueue_USART0.maxLength = USART0_TX_BUFFER_SIZE;
    txQueue_USART0.queueLength = 0;
    txQueue_USART0.empty = TRUE;
    txQueue_USART0.full = FALSE;
    txQueue_USART0.txOnEnqueue = TRUE;
    txQueue_USART0.in = 0;
    txQueue_USART0.out = 0;
    txQueue_USART0.txOn = TX_ON_USART0;

} // initUSART0

// queue an array of bytes for transmission
void transmitBufferUSART0(uint8_t const buffer[], uint8_t buffer_length)
{
    uint8_t x;
    for (x = 0; x < buffer_length; x++)
    {
        enqueue(buffer[x], &txQueue_USART0);
    }
} // transmitBufferUSART0

void transmitBufferUSART0_busyLoop(uint8_t const buffer[], uint8_t buffer_length)
{
    uint8_t x;
    for(x=0; x<buffer_length; x++)
        transmitByteUSART0_busyLoop(buffer[x]);
}

// queue a single byte for transmission
void transmitByteUSART0(uint8_t msg)
{
    enqueue(msg, &txQueue_USART0);
}

void transmitByteUSART0_busyLoop(char msg)
{
    txdLEDOn();
    // wait for the buffer to empty
    while( !(UCSR0A & (1 << UDRE0)) )
        asm volatile("nop"::);

    // send the byte
    UDR0 = msg;

    while( !(UCSR0A & (1 << UDRE0)) )
        asm volatile("nop"::);

    txdLEDOff();
}

void transmitPacketUSART0(DXL_Packet *packet)
{
    uint8_t i;

    enqueue(packet->header1, &txQueue_USART0);
    enqueue(packet->header2, &txQueue_USART0);
    enqueue(packet->id, &txQueue_USART0);
    enqueue(packet->paramLength+2, &txQueue_USART0);
    enqueue(packet->instrErr, &txQueue_USART0);

    for(i=0; i<packet->paramLength; i++)
        enqueue(packet->params[i], &txQueue_USART0);

    enqueue(packet->checksum, &txQueue_USART0);
}

// dequeue a byte from the Rx queue and return it
uint8_t getByteUSART0()
{
    return dequeue(&rxQueue_USART0);
}


// monitor responses from motors/sensors connected to USART0 and store them in responsePacketBuffer
// response packet is:
//     0xff 0xff id length error parameter1 parameter2 [... parameterN] checksum
// where checksum = ~(id + err + position_lo + position_hi)
// fires STATUS_PACKET_RECEIVED_EVENT when the packet is fully received (regardless of checksum)
void monitorUSART0(void* arg)
{
    uint8_t byte;           // latest packet dequeued from USART0

    // how many parameters have we received from the packet so far?
    uint8_t paramsRecv = 0;

    // what is the current state of the received packet? (i.e. what was the last byte received?)
    uint8_t state = USART0_INITIAL_STATE;

    // continually scan the USART0 Rx buffer, and collect bytes into an intermediate buffer
    for(;;)
    {
        lockSemaphore(&usart0_lock);

        if(!rxQueue_USART0.empty)
        {
            lockSemaphore(&responsePacketLock);

            byte = dequeue(&rxQueue_USART0);

            // change the state depending on the byte we just read
            switch(state)
            {
            case USART0_INITIAL_STATE:
                if(byte == USART0_PACKET_HEADER0)
                {
                    responsePacket.header1 = byte;
                    state = USART0_HEADER1_STATE;
                }
                break;

            case USART0_HEADER1_STATE:
                if(byte == USART0_PACKET_HEADER1)
                {
                    responsePacket.header2 = byte;
                    state = USART0_HEADER2_STATE;
                }
                else
                {
                    // fall back to the initial state if we don't get the FF FF header
                    state = USART0_INITIAL_STATE;
                }
                break;

            case USART0_HEADER2_STATE:
                responsePacket.id = byte;
                state = USART0_ID_STATE;
                break;

            case USART0_ID_STATE:
                responsePacket.paramLength = byte - 2;  // packet length = # of params + 2
                state = USART0_LENGTH_STATE;
                break;

            case USART0_LENGTH_STATE:
                responsePacket.instrErr = byte;
                paramsRecv = 0;
                state = USART0_ERROR_STATE;
                break;

            case USART0_ERROR_STATE:
            case USART0_PARAMETER_STATE:
                // we received pLength-2 parameters, then the checksum
                if(responsePacket.paramLength == paramsRecv)
                {
                    // checksum
                    responsePacket.checksum = byte;

                    // announce that the packet is fully received
                    fireEvent(STATUS_PACKET_RECEIVED_EVENT);

                    // drop back to the initial state and wait for the next packet
                    state = USART0_INITIAL_STATE;

                }
                else
                {
                    // new parameter
                    responsePacket.params[paramsRecv] = byte;
                    paramsRecv++;
                    state = USART0_PARAMETER_STATE;
                }
                break;

            default:
                // we're in an undefined state so just fall back to default
                state = USART0_INITIAL_STATE;
                break;
            }//switch

            unlockSemaphore(&responsePacketLock);
            unlockSemaphore(&usart0_lock);

#ifdef DEBUG
            // debugging
            lockSemaphore(&usart1_tx_lock);
            transmitByteUSART1(byte);
            transmitByteUSART1(rxQueue_USART0.queueLength);
            unlockSemaphore(&usart1_tx_lock);
#else
            // re-transmit the status packet back over USART 1 to the main board
            lockSemaphore(&usart1_tx_lock);
            lockSemaphore(&responsePacketLock);
            transmitPacketUSART1(&responsePacket);
            unlockSemaphore(&responsePacketLock);
            unlockSemaphore(&usart1_tx_lock);
#endif
        }// !isEmpty
        else
        {
            // the buffer is empty, so we can sleep until it hears something
            unlockSemaphore(&usart0_lock);
            wait(USART0_RX_EVENT);
            //sleep(1);
        }

    }// for
}

// interrupt handler for receiving a byte
// puts the byte in the Rx queue
ISR(USART0_RX_vect)
{
    uint8_t data = UDR0;

    enqueue(data, &rxQueue_USART0);

    fireEvent(USART0_RX_EVENT);
}

// interrupt handler for transmitting a byte
// dequeues the next byte from the Tx queue and
// transmits it, falling back to Rx mode when
// the queue is empty
ISR(USART0_TX_vect)
{
    // dequeue a single byte of data to send
    if(!txQueue_USART0.empty)
    {
        uint8_t data = dequeue(&txQueue_USART0);
        UDR0 = data;
    }
    else
    {
        txQueue_USART0.txOnEnqueue = TRUE;
    }

    fireEvent(USART0_TX_EVENT);
}
